// 引入 AABBOriginGenerator 的头文件，定义 AABBCenterOriginGenerator 类
#include <noether_tpp/tool_path_planners/raster/origin_generators/aabb_origin_generator.h>

// 引入 PCL（Point Cloud Library）的常用函数头文件，提供点云处理的相关功能
#include <pcl/common/common.h>
// 引入 PCL 点云转换相关的头文件，用于将不同格式的点云数据进行转换
#include <pcl/conversions.h>

namespace noether
{
    // AABBCenterOriginGenerator 类的成员函数 generate，返回一个 Eigen::Vector3d 类型的结果
    // 该函数接收一个 pcl::PolygonMesh 类型的点云数据，计算并返回该点云的 AABB（轴对齐边界框）中心
    Eigen::Vector3d AABBCenterOriginGenerator::generate(const pcl::PolygonMesh& mesh) const
    {
        // 创建一个 pcl::PointCloud<pcl::PointXYZ> 类型的点云对象，用于存储从 mesh 提取的点
        pcl::PointCloud<pcl::PointXYZ> vertices;

        // 将 pcl::PolygonMesh 类型的点云数据（mesh.cloud）转换为 pcl::PointCloud<pcl::PointXYZ> 类型（vertices）
        pcl::fromPCLPointCloud2(mesh.cloud, vertices);

        // 定义 min 和 max 点，分别表示点云中的最小和最大点
        pcl::PointXYZ min, max;

        // 使用 PCL 函数计算点云中的最小值和最大值，分别赋值给 min 和 max
        pcl::getMinMax3D(vertices, min, max);

        // 计算 AABB 的范围，max 和 min 是 pcl::PointXYZ 类型，调用 getArray3fMap() 获取到浮动点数组
        // 然后将其转换为 Eigen::Vector3d 类型，计算 max 和 min 之间的差值（范围）
        Eigen::Vector3d range = (max.getArray3fMap() - min.getArray3fMap()).matrix().cast<double>();

        // 返回 AABB 中心点的坐标，即范围的一半
        // 由于 AABB 的中心点是 (max + min) / 2，所以返回范围的一半作为中心点
        return range / 2.0;
    }

}  // namespace noether